// standard libs
#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>


#include "ff_utils.h"
#include "main.h"
#include "../utils/util.h"
#include "../utils/button.h"
#include "../utils/LCD_driver.h"
#include "../utils/LCD_functions.h"

/**********************************************************************************************************
*
* 	Forward - Goes forward for a time
*
***********************************************************************************************************/

void Forward(int Speed,int TerminatingCondition,unsigned long TerminatingValue)
{
	do
	{
		SetLeftMotorPWM(Speed);
		SetRightMotorPWM(Speed);
	} while(!CheckTerminatingConditions(TerminatingCondition,TerminatingValue));

	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
}


//********************
//*	GoToHeading()	*
//********************/
int GoToHeading(unsigned char TargetHeading,unsigned char Speed)
{
	if(ReadCompass())
	{
		if(Heading == TargetHeading)
		{
			return(TRUE);
		}
		//Left turns that don't cross zero
		if((Heading > TargetHeading) && ((Heading - TargetHeading) <= 128))
		{	
			while((Heading > TargetHeading) && ((Heading -TargetHeading)>2))
			{//Turn at decreasing speed until within 2 of Target
				SetLeftMotorPWM(-(Heading - TargetHeading)/20);
				SetRightMotorPWM((Heading - TargetHeading)/20);
				ReadCompass();
			}
			SetRightMotorPWM(0);//Stop
			SetLeftMotorPWM(0);
			TimerWait(10);
			return(TRUE);//Exit
		}	
		//Left turns that cross zero 
		if	((Heading < TargetHeading) && ((TargetHeading - Heading) >= 128))
		{
			while((Heading < TargetHeading) && ((TargetHeading - Heading)>2))
			{//Turn at decreasing speed until within 2 of traget
			
				SetLeftMotorPWM(-(TargetHeading - Heading)/20);
				SetRightMotorPWM((TargetHeading - Heading)/20);
				ReadCompass();
			}
			SetLeftMotorPWM(0);
			SetRightMotorPWM(0);		
			TimerWait(10);
			return(TRUE);
		}
		//Right Turns
		//Right turn that do not cross zero
		if((Heading < TargetHeading) && ((TargetHeading - Heading) < 128))
		{
			while((Heading < TargetHeading) && ((TargetHeading - Heading)>2))
			{//Turn at descreasing speed until within 2 of Target
				SetLeftMotorPWM((TargetHeading - Heading)/20);
				SetRightMotorPWM(-(TargetHeading - Heading)/20);		
				ReadCompass();
			}
			SetRightMotorPWM(0);//Stop
			SetLeftMotorPWM(0);
			TimerWait(10);
			return(TRUE);//exit
		}	
		//right turn that do cross zero
		if	((Heading > TargetHeading) && ((Heading - TargetHeading) > 128))
		{
			while((Heading > TargetHeading) && ((Heading - TargetHeading)>2))
			{//Turn with decreasing speed until within 2 of target
				SetLeftMotorPWM((Heading - TargetHeading)/20);
				SetRightMotorPWM(-(Heading - TargetHeading)/20);
				ReadCompass();
			}
			SetLeftMotorPWM(0);//stop
			SetRightMotorPWM(0);		
			TimerWait(10);
			return(TRUE);//exit
		}

		SetRightMotorPWM(0);
		SetLeftMotorPWM(0);
		TimerWait(10);
		return(TRUE);
		
	}
	else
	{
		return(FALSE);
	}
}


void RTurn90old(void)
{
	unsigned char	TargetHeading;

	TargetHeading = ReadCompass(); 
	if (ReadCompass())
	{
		Heading = Heading + 64;
	
		while (!(getkey() == 1))
		{
			if(!GoToHeading(TargetHeading,10))
			{
				LCD_puts_f(PSTR("Error"),1);
			}
		}
	}	
}




int ScanRoomForFlame(void)
//this will see if there is flame in the room
//changed to do 540 degrees

{
	unsigned char	EntryHeading,
					HotPixel,
					retries,
					Temperature,
					TargetHeading;
//					index;
	int 			NumberOfTurns;
	
	
	// get entry heading
	retries	= 0;
	while(!ReadCompass())
	{
		if(++retries > 5)
			return(FALSE);
	}

	EntryHeading	=
	TargetHeading	= Heading;
	NumberOfTurns	= 0;
	
	while (!(getkey() == 1))
	{
		// look for hot pixel
		GetHotPixel(&HotPixel,&Temperature);
		//TimerWait(200);
		if(Temperature > 6) // If there is a flame, returns true
		{
			LCD_puts_f(PSTR("Sees Flame"),1);
			TimerWait(10000);				
			show2hex(HotPixel,Temperature);
		    TimerWait(2000);		
			sendNumber(HotPixel);
			sendChar(',');
			sendNumber(Temperature);
			sendString("\r\n");
			TargetHeading	= TargetHeading + ((4 - HotPixel) * 3); // try to leave function centered on flame
			GoToHeading(TargetHeading,8);
//			GoToFlame();
			return(TRUE);
		}
		else
		{
			TargetHeading	= TargetHeading + 14; // 28 is 40 degrees, range of the sensor	
			GoToHeading(TargetHeading,8);
			NumberOfTurns++;
			
			if(NumberOfTurns >= 20) // If a full turn is completed, return to orignal heading
			{
				GoToHeading(EntryHeading + 128,8);
				LCD_puts_f(PSTR("No Flame"),1);
				TimerWait(2000);					
				return(FALSE);
			}
		}
	}
	return(FALSE);
}


/**********************************************************************************************************
###########################################################################################################
###########################################################################################################
###########################################################################################################
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 	

Going To The Rooms (Canada)

^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
###########################################################################################################
###########################################################################################################
###########################################################################################################
***********************************************************************************************************/




/**********************************************************************************************************
*
* 	Goes To Room 1 Canada
*
***********************************************************************************************************/

//#########################################################################################################
void ToRoom1(void)
{
	Forward(10,UNTIL_TIME,TimerMsCur() + 1000);
	RWallFollow(0x3D,UNTIL_LINESENSOR_LOWER,0x40); 
	/*GoToHeading((RoomOneEnterHeading + 8), 8); //132
	LCD_puts_f(PSTR("SQUARE"),1);
	*/TimerWait(100);
	Forward(8,UNTIL_TIME,TimerMsCur() + 1000);	
	LCD_puts_f(PSTR("SCAN ROOM"),1);
	TimerWait(100);

	if(ScanRoomForFlame())
	{	
		GoToFlame();
		SetLeftMotorPWM(0);
		SetRightMotorPWM(0);
		TimerWait(10000);
		HomeFrom1();
	}
	else
	{
		LCD_puts_f(PSTR("Back from Check for flame"),1);	
		TimerWait(1000);
		GoToHeading((RoomOneEnterHeading + 112 ), 8);
		LCD_puts_f(PSTR("Org Heading"),1);
		TimerWait(1000);		
		Forward(8,UNTIL_LINESENSOR_LOWER,0x40);
		// goes To Room 2 from room 1 door white line
		ToRoom2();
	}
	LCD_puts_f(PSTR("Exit"),1);
}
//#########################################################################################################

/**********************************************************************************************************
*
* 	Goes To Room 2 Canada
*
***********************************************************************************************************/

//#########################################################################################################
void ToRoom2(void)//go to room 2 from room 1
{
	Forward(8,UNTIL_TIME,TimerMsCur() + 700);
	RWallFollow(0x20,UNTIL_LINESENSOR_LOWER,0x40);// follows right wall untill line sensor sees doorway 
	GoToHeading((RoomTwoEnterHeading - 8), 7);
	Forward(8,UNTIL_TIME,TimerMsCur() + 1000);
	LCD_puts_f(PSTR("SQUARE"),1);
	TimerWait(1000);	
	LCD_puts_f(PSTR("SCAN ROOM"),1);
	TimerWait(1000);

	if(ScanRoomForFlame())
	{
		GoToFlame();
		HomeFrom2();
/*		unsigned char	FlameHeading	= Heading;

		GoToFlame();
		/// *** works? check or replace with home from 2
		Forward(-8,UNTIL_TIME,TimerMsCur() + 1000);
		TimerWait(2000);
		GoToHeading(FlameHeading + 128, 8);
		TimerWait(2000);
		LWallFollow(0x30,UNTIL_LINESENSOR_LOWER,0x40);// line sensor   when left sees doorway 
		TimerWait(2000);
		GoToHeading(OriginalHeading + 32, 8);
		TimerWait(2000);
		Forward(8,UNTIL_LEFTSENSOR_GREATER,0x30);
		TimerWait(2000);
		LWallFollow(0x30,UNTIL_LINESENSOR_LOWER,0x40);// line sensor   when left sees doorway
		TimerWait(2000);
		Forward(8,UNTIL_TIME,TimerMsCur() + 500); 
		TimerWait(2000);
		LWallFollow(0x30,UNTIL_LINESENSOR_LOWER,0x40);// line sensor   when left sees doorway 
		//Go Home From Room 1
		TimerWait(2000);*/
	}
	else
	{
		LCD_puts_f(PSTR("Back from Check for flame"),1);	
		TimerWait(500);
		GoToHeading((RoomTwoEnterHeading+ 132), 7);
		TimerWait(500);		
		Forward(8,UNTIL_LINESENSOR_LOWER,0x40);
		//LCD_puts_f(PSTR("Done"),1);
		//TimerWait(1000);
		ToRoom3();
	}
	LCD_puts_f(PSTR("Exit"),1);
}		
//#########################################################################################################

/**********************************************************************************************************
*
* 	Goes To Room 3 Canada
*
***********************************************************************************************************/

//#########################################################################################################
void ToRoom3(void)//go to room 3 from room 2
{
    Forward(8,UNTIL_TIME,TimerMsCur() + 500);
	RWallFollow(0x20,UNTIL_LINESENSOR_LOWER,0x40);
	TimerWait(1000);
    Forward(8,UNTIL_TIME,TimerMsCur() + 100);	
	GoToHeading((RoomThreeEnterHeading), 8);
	// *** change to lower angle to center in doorway
	//TimerWait(1000);
	Forward(8, UNTIL_MIDDLESENSOR_GREATER,0x25);
	if (ScanRoomForFlame())
	{
		GoToFlame();
		HomeFrom3(); 
	}
	else
	{
		GoToHeading((RoomThreeEnterHeading +136), 8);
		//TimerWait(1000);
		Forward(8, UNTIL_LINESENSOR_LOWER, 0x40);// maybe follow right wall ***************
		//LCD_puts_f(PSTR("EXIT R3"),1);
		//TimerWait(1000);
		ToRoom4();
	}
	LCD_puts_f(PSTR("Exit"),1);

}
//#########################################################################################################

/**********************************************************************************************************
*
* 	Goes To Room 4 Canada
*
***********************************************************************************************************/

//#########################################################################################################
void ToRoom4(void)//go to room 4 from room 3
{
	GoToHeading((RoomThreeEnterHeading -120), 8);
	Forward(8, UNTIL_TIME, TimerMsCur() + 200);
	RWallFollow(0x37, UNTIL_LINESENSOR_LOWER, 0x40);
	GoToHeading((RoomFourEnterHeading - 8), 8);	
	Forward(8, UNTIL_TIME, TimerMsCur() + 1000);
	if (ScanRoomForFlame())
	{
		GoToFlame();
		HomeFrom4();
	}
	else
	{
	    GoToHeading((RoomFourEnterHeading+128), 8);	
		Forward(8, UNTIL_LINESENSOR_LOWER, 0x40);	
		Forward(8, UNTIL_TIME, TimerMsCur() + 100);	
		RWallFollow(0x30,UNTIL_LINESENSOR_LOWER,0x40);// line sensor   when left sees doorway 
		TimerWait(100);
		Forward(8,UNTIL_TIME,TimerMsCur() + 500); 
		TimerWait(100);
	}
	LCD_puts_f(PSTR("Exit"),1);	
}	
//#########################################################################################################

/**********************************************************************************************************
*
* 	Goes Home from Room 1 Canada
*
***********************************************************************************************************/

//#########################################################################################################
 void HomeFrom1(void)
{
	Forward(-8,UNTIL_TIME,TimerMsCur() + 500);
	LCD_puts_f(PSTR("Exiting"),1);		
	ExitRoom();
	GoToHeading((RoomOneEnterHeading +128),8);	
	TimerWait(1000);
	Forward(10,UNTIL_TIME,TimerMsCur() + 500);	
	RWallFollow(0x30,UNTIL_TIME,TimerMsCur() + 2000);//	
	LCD_puts_f(PSTR("2500"),1);		
	LWallFollow(0x30,UNTIL_LINESENSOR_LOWER,0x40);// line sensor   when left sees doorway 
	TimerWait(100);
	Forward(8,UNTIL_TIME,TimerMsCur() + 500); 
	TimerWait(10000);
}
//#########################################################################################################

/**********************************************************************************************************
*
* 	Goes Home from Room 2 Canada
*
***********************************************************************************************************/

//#########################################################################################################
 void HomeFrom2(void)
{
	Forward(-8,UNTIL_TIME,TimerMsCur() + 500);
	LCD_puts_f(PSTR("Exiting"),1);		
	ExitRoom();
	GoToHeading((RoomTwoEnterHeading +128),8);	
	TimerWait(1000);
	Forward(10,UNTIL_TIME,TimerMsCur() + 500);	
	LWallFollow(0x30,UNTIL_LINESENSOR_LOWER,0x40);// line sensor   when left sees doorway 
	TimerWait(100);
	Forward(8,UNTIL_TIME,TimerMsCur() + 500); 
	TimerWait(10000);
}
//#########################################################################################################

/**********************************************************************************************************
*
* 	Goes Home from Room 3 Canada
*
***********************************************************************************************************/


//#########################################################################################################
 void HomeFrom3(void)
{
	Forward(-8,UNTIL_TIME,TimerMsCur() + 500);
	LCD_puts_f(PSTR("Exiting"),1);		
	ExitRoom();
	GoToHeading((RoomThreeEnterHeading +128),8);	
	TimerWait(1000);
	Forward(10,UNTIL_TIME,TimerMsCur() + 500);	
	RWallFollow(0x30,UNTIL_LINESENSOR_LOWER,0x40);// line sensor   when left sees doorway 
	GoToHeading((RoomFourEnterHeading +150),8);
	Forward(10,UNTIL_TIME,TimerMsCur() + 800);
	RWallFollow(0x30,UNTIL_LINESENSOR_LOWER,0x40);	
	TimerWait(100);
	Forward(8,UNTIL_TIME,TimerMsCur() + 500); 
	TimerWait(10000);
}
//########################################################################################################



/**********************************************************************************************************
*
* 	Goes Home from Room 4 Canada
*
***********************************************************************************************************/


//#########################################################################################################
 void HomeFrom4(void)
{
	Forward(-8,UNTIL_TIME,TimerMsCur() + 500);
	LCD_puts_f(PSTR("Exiting"),1);		
	ExitRoom();
	GoToHeading((RoomFourEnterHeading +136),8);	
	TimerWait(1000);
	Forward(10,UNTIL_TIME,TimerMsCur() + 700);	
	RWallFollow(0x30,UNTIL_LINESENSOR_LOWER,0x40);// line sensor   when left sees doorway 
	Forward(10,UNTIL_TIME,TimerMsCur() + 500);
	TimerWait(100);
	Forward(8,UNTIL_TIME,TimerMsCur() + 500); 
	TimerWait(10000);
}
//#########################################################################################################


/**********************************************************************************************************
*
* 	Leaves the room
*
***********************************************************************************************************/

void ExitRoom(void)
{

	while (GetADC(FrontLineSensor) > 0x80) // Drive until line on the floor
	{	

	//Turn sharp if too close to wall
		if ((GetADC(LeftDistSensor) > 0x38))
		{	
			SetLeftMotorPWM(10);
			SetRightMotorPWM(-7);
		}
		else if ((GetADC(RightDistSensor) > 0x38))
		{
			SetLeftMotorPWM(-7);
			SetRightMotorPWM(10);
		}
	//Steer out of the room	
		else if (GetADC(LeftDistSensor) > (GetADC(RightDistSensor)))	
		{	
			SetLeftMotorPWM(10);
			SetRightMotorPWM(5);		
		}
		else 
		{					
			SetLeftMotorPWM(5);
			SetRightMotorPWM(10);
		}
	}
//Reached the threshold
	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);	
}



/**********************************************************************************************************
*
* 	Follows Right Wall
*
***********************************************************************************************************/


void RWallFollow(int TargetDistance,int TerminatingCondition,unsigned long TerminatingValue)
{
	unsigned char adcL, adcM, adcR;
   
	do
	{
		adcR = GetADC(RightDistSensor);
		adcM = GetADC(MiddleDistSensor);
		adcL = GetADC(LeftDistSensor);
		//sendNumber(adcL);
		//TimerWait(1);
		//sendChar(',');
		//TimerWait(1);
		//sendNumber(adcM);
		//TimerWait(1);
		//sendChar(',');
		//TimerWait(1);
		//sendNumber(adcR);
		//TimerWait(5);
		//sendString("\r\n");
		//TimerWait(50);
		//if(adcM > 0x2A)
		//{
		//	SetLeftMotorPWM(3);
		//	SetRightMotorPWM(10);
		//}			
/*		else if(adcR > (TargetDistance + 10))
		{
			SetLeftMotorPWM(4);
			SetRightMotorPWM(10);
		}	
		if(adcR > (TargetDistance + 8))
		{
			SetLeftMotorPWM(6);
			SetRightMotorPWM(10);
		}*/
		if(adcR > (TargetDistance + 6))
		{
			printLCD("37");
			SetLeftMotorPWM(3);
			SetRightMotorPWM(7);
		}		
		else if(adcR > (TargetDistance + 4))
		{
			printLCD("77");
			SetLeftMotorPWM(7);
			SetRightMotorPWM(7);
		}
/*		else if(adcR > (TargetDistance + 2))
		{
			SetLeftMotorPWM(7);
			SetRightMotorPWM(7);
		}	
		else if(adcR < (TargetDistance - 10))
		{
			SetLeftMotorPWM(10);
			SetRightMotorPWM(4);	
		}			
		else if(adcR < (TargetDistance - 8))
		{
			SetLeftMotorPWM(10);
			SetRightMotorPWM(6);	
		}	
		else if(adcR < (TargetDistance - 6))
		{
			SetLeftMotorPWM(7);
			SetRightMotorPWM(3);	
		}*/
		else if(adcR < (TargetDistance - 4))
		{
			printLCD("73");
			SetLeftMotorPWM(7);
			SetRightMotorPWM(3);
		}		
		else if(adcR < (TargetDistance - 2))
		{
			printLCD("77");
			SetLeftMotorPWM(7);
			SetRightMotorPWM(7);
		}
		else
		{
			SetLeftMotorPWM(7);
			SetRightMotorPWM(7);
		}
		


	} while(!CheckTerminatingConditions(TerminatingCondition,TerminatingValue));
	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
}


/**********************************************************************************************************
*
* 	Follows Left Wall
*
***********************************************************************************************************/



void LWallFollow(int TargetDistance,int TerminatingCondition,unsigned long TerminatingValue)
{
	unsigned char adcL, adcM, adcR;
   
	do
	{
		adcR = GetADC(RightDistSensor);
		adcM = GetADC(MiddleDistSensor);
		adcL = GetADC(LeftDistSensor);
		//show2(adcL, adcM);
		sendNumber(adcL);
		sendChar(',');
		sendNumber(adcM);
		sendChar(',');
		sendNumber(adcR);
		sendString("\r\n");
		if(adcM > 0x25)
		{
			SetLeftMotorPWM(10);
			SetRightMotorPWM(3);
		}
//		if(adcL < (TargetDistance - 10))
//		{
//			SetLeftMotorPWM(4);
//			SetRightMotorPWM(10);
//		}		
		else if(adcL < (TargetDistance - 8))
		{
			SetLeftMotorPWM(6);
			SetRightMotorPWM(10);
		}				
		else if(adcL < (TargetDistance - 6))
		{
			SetLeftMotorPWM(8);
			SetRightMotorPWM(10);
		}		
		else if(adcL < (TargetDistance - 4))
		{
			SetLeftMotorPWM(9);
			SetRightMotorPWM(10);
		}
		else if(adcL < (TargetDistance - 2))
		{
			SetLeftMotorPWM(10);
			SetRightMotorPWM(10);
		}
//		else if(adcL > (TargetDistance + 10))
//		{
//			SetLeftMotorPWM(10);
//			SetRightMotorPWM(4);
//		}
		else if(adcL > (TargetDistance + 8))
		{
			SetLeftMotorPWM(10);
			SetRightMotorPWM(6);
		}		
		else if(adcL > (TargetDistance + 6))
		{
			SetLeftMotorPWM(10);
			SetRightMotorPWM(8);
		}
		else if(adcL > (TargetDistance + 4))
		{
			SetLeftMotorPWM(10);
			SetRightMotorPWM(9);
		}
		else if(adcL > (TargetDistance + 2))
		{
			SetLeftMotorPWM(10);
			SetRightMotorPWM(10);
		}		
		else
		{
			SetLeftMotorPWM(10);
			SetRightMotorPWM(10);
		}

	} while(!CheckTerminatingConditions(TerminatingCondition,TerminatingValue));
	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
}





/**********************************************************************************************************
*
* 	A FireFighting Demo For Showing Off
*
***********************************************************************************************************/

void FlameDemo(void)
{
	while (!(getkey() == 1))
	{
	unsigned char	HotPixel,
					Temperature;
		GetHotPixel(&HotPixel,&Temperature);

		if(Temperature > 6) // If there is a flame, returns true
		{
			show2hex(HotPixel,Temperature);
			sendNumber(HotPixel);
			sendChar(',');
			sendNumber(Temperature);
			sendString("\r\n");
			if (HotPixel == 1)
			{
				//LCD_puts_f(PSTR("pixel 1"),1);
				//SetRightMotorPWM(0);
				//SetLeftMotorPWM(0);
				//TimerWait(1000);
				SetRightMotorPWM(7);
				SetLeftMotorPWM(10);
				TimerWait(10);
				sendString("Pixel 1");	
				sendChar(',');
				sendNumber(Temperature);	
				sendString("\r\n");
			}
			else if (HotPixel == 2 )
			{
				//LCD_puts_f(PSTR("pixel 2"),1);
				//SetRightMotorPWM(0);
				//SetLeftMotorPWM(0);
				//TimerWait(1000);
				SetRightMotorPWM(8);
				SetLeftMotorPWM(10);
				TimerWait(10);
				sendString("Pixel 2");	
				sendChar(',');
				sendNumber(Temperature);	
				sendString("\r\n");
			}
			else if (HotPixel == 3 )
			{
			//	LCD_puts_f(PSTR("pixel 3"),1);
			//	SetRightMotorPWM(0);
			//	SetLeftMotorPWM(0);		
			//	TimerWait(1000);
				SetRightMotorPWM(9);
				SetLeftMotorPWM(10);
				TimerWait(10);
				sendString("Pixel 3");	
				sendChar(',');
				sendNumber(Temperature);	
				sendString("\r\n");			
			}			
			else if (HotPixel == 4 )
			{
			//	LCD_puts_f(PSTR("pixel 4"),1);
				//SetRightMotorPWM(0);
			//	SetLeftMotorPWM(0);
			//	TimerWait(1000);
				SetRightMotorPWM(10);
				SetLeftMotorPWM(10);
				TimerWait(10);
				sendString("Pixel 4");	
				sendChar(',');
				sendNumber(Temperature);	
				sendString("\r\n");
			}						
			else if (HotPixel == 5 )
			{
			//	LCD_puts_f(PSTR("pixel 5"),1);
				//SetRightMotorPWM(0);
				//SetLeftMotorPWM(0);
				//TimerWait(1000);
				SetRightMotorPWM(10);
				SetLeftMotorPWM(10);
				TimerWait(10);
				sendString("Pixel 5");	
				sendChar(',');
				sendNumber(Temperature);	
				sendString("\r\n");
			}
			else if (HotPixel == 6)
			{
				//LCD_puts_f(PSTR("pixel 6"),1);
				//SetRightMotorPWM(0);
				//SetLeftMotorPWM(0);
				//TimerWait(1000);
				SetRightMotorPWM(10);
				SetLeftMotorPWM(9);
				TimerWait(10);
				sendString("Pixel 6");	
				sendChar(',');
				sendNumber(Temperature);	
				sendString("\r\n");
			}						
			else if (HotPixel == 7)
			{
				//LCD_puts_f(PSTR("pixel 7"),1);
				//SetRightMotorPWM(0);
				//SetLeftMotorPWM(0);
				//TimerWait(1000);
				SetRightMotorPWM(10);
				SetLeftMotorPWM(8);
				TimerWait(10);
				sendString("Pixel 7");	
				sendChar(',');
				sendNumber(Temperature);	
				sendString("\r\n");
			}		
			else if (HotPixel == 8)
			{
				//LCD_puts_f(PSTR("pixel 8"),1);
				//SetRightMotorPWM(0);
				//SetLeftMotorPWM(0);
				//TimerWait(1000);
				SetRightMotorPWM(10);
				SetLeftMotorPWM(7);
				TimerWait(10);
				sendString("Pixel 8");	
				sendChar(',');
				sendNumber(Temperature);	
				sendString("\r\n");
			}

			/*if(PINB & 0x20)				// check for balloon pop
			{
				SetRightMotorPWM(0);
				SetLeftMotorPWM(0);
				TimerWait(1000);
				break;
			*/

			else
			{
				break;
			}
		}
		else
		{
			SetRightMotorPWM(7);
			SetLeftMotorPWM(-7);
			LCD_puts_f(PSTR("no"),1);
			sendString("No");		
			sendString("\r\n");
		}
	}
}




/**********************************************************************************************************
*
* 	Terminating Conditions - a while loop with diffrent terminating conditions.
*
***********************************************************************************************************/

int CheckTerminatingConditions(int TerminatingCondition,unsigned long TerminatingValue)
{
	unsigned char adcL, adcM, adcR;

	adcR = GetADC(RightDistSensor);
	adcM = GetADC(MiddleDistSensor);
	adcL = GetADC(LeftDistSensor);

	switch(TerminatingCondition)
	{
	case UNTIL_KEYPRESS:
		if(getkey() == TerminatingValue)
			return(TRUE);
		
		break;

	case UNTIL_RIGHTSENSOR_GREATER:
		if(GetADC(RightDistSensor) > TerminatingValue)
			return(TRUE);
		
		break;

	case UNTIL_MIDDLESENSOR_GREATER:
		if(GetADC(MiddleDistSensor) > TerminatingValue)
			return(TRUE);
		
		break;

	case UNTIL_LEFTSENSOR_GREATER:
		if(GetADC(LeftDistSensor) > TerminatingValue)
			return(TRUE);
		
		break;
	
	case UNTIL_LINESENSOR_GREATER:
		if(GetADC(FrontLineSensor) > TerminatingValue)
			return(TRUE);
		
		break;

	case UNTIL_RIGHTSENSOR_LOWER:
		if(GetADC(RightDistSensor) < TerminatingValue)
			return(TRUE);
		
		break;

	case UNTIL_MIDDLESENSOR_LOWER:
		if(GetADC(MiddleDistSensor) < TerminatingValue)
			return(TRUE);
		
		break;

	case UNTIL_LEFTSENSOR_LOWER:
		if(GetADC(LeftDistSensor) < TerminatingValue)
			return(TRUE);
		
		break;
	
	case UNTIL_LINESENSOR_LOWER:
		if(GetADC(FrontLineSensor) < TerminatingValue)
			return(TRUE);
		
		break;

	case UNTIL_TIME:
		if(TimerMsCur() > TerminatingValue)
			return(TRUE);
		
		break;
	}

	return(FALSE);

}




/**********************************************************************************************************
###########################################################################################################
###########################################################################################################
###########################################################################################################
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 	

Fire Fighting Tests

^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
###########################################################################################################
###########################################################################################################
###########################################################################################################
***********************************************************************************************************/




/**********************************************************************************************************
*
* 	Threshold Test - Tests the Line Sensor by driving and sending out the sensor value.
*
***********************************************************************************************************/

//##########################################################################################################
void Thresholdtest(void)
{
	
	unsigned char adcLine;
	
    while (!(getkey() == 1))
    { 
	SetLeftMotorPWM(10);
    SetRightMotorPWM(10);
	adcLine = GetADC(FrontLineSensor);
	show1hex(adcLine);	
	sendNumber(adcLine);
	sendString("\r\n");
	TimerWait(100);
	}

}
//##########################################################################################################





/**********************************************************************************************************
*
*	 Motor Test - Tests the Motors By Driving them in diffrent directions.
*
***********************************************************************************************************/

//##########################################################################################################
void PWM_Test(void)
{
    while (!(getkey() == 1))
    { 	 
	    LCD_puts_f(PSTR("Left"),1);
	    SetLeftMotorPWM(-10);
        SetRightMotorPWM(10);
        TimerWait(3000);
		
		LCD_puts_f(PSTR("Right"),1);	
	    SetLeftMotorPWM(10);
        SetRightMotorPWM(-10);
        TimerWait(3000);
		
        LCD_puts_f(PSTR("2"),1);
	    SetLeftMotorPWM(2);
        SetRightMotorPWM(2);
        TimerWait(3000);
		
		LCD_puts_f(PSTR("4"),1);	
	    SetLeftMotorPWM(4);
        SetRightMotorPWM(4);
        TimerWait(3000);
	 
        LCD_puts_f(PSTR("6"),1);
	    SetLeftMotorPWM(6);
        SetRightMotorPWM(6);
        TimerWait(3000);
		
		LCD_puts_f(PSTR("8"),1);	
	    SetLeftMotorPWM(8);
        SetRightMotorPWM(8);
        TimerWait(3000);
	 
        LCD_puts_f(PSTR("10"),1);
	    SetLeftMotorPWM(10);
        SetRightMotorPWM(10);
        TimerWait(3000);
	
		LCD_puts_f(PSTR("-2"),1);	
	    SetLeftMotorPWM(-2);
        SetRightMotorPWM(-2);
        TimerWait(3000);
	 
        LCD_puts_f(PSTR("-4"),1);
	    SetLeftMotorPWM(-4);
        SetRightMotorPWM(-4);
        TimerWait(3000);

		LCD_puts_f(PSTR("-6"),1);	
	    SetLeftMotorPWM(-6);
        SetRightMotorPWM(-6);
        TimerWait(3000);
	 
        LCD_puts_f(PSTR("-8"),1);
	    SetLeftMotorPWM(-8);
        SetRightMotorPWM(-8);
        TimerWait(3000);
		
		LCD_puts_f(PSTR("-10"),1);	
	    SetLeftMotorPWM(-10);
        SetRightMotorPWM(-10);
        TimerWait(3000);
	}
    // Done
		SetLeftMotorPWM(0);
        SetRightMotorPWM(0);
}
//##########################################################################################################




/**********************************************************************************************************
*
*	 Heading test - Turns to diffrent points using the compass
*
***********************************************************************************************************/

//##########################################################################################################
void HeadingTest(void) 
{
	unsigned char TargetHeading = 32;
	GoToHeading(TargetHeading,10);
	TimerWait(100);
	
	GoToHeading(TargetHeading,32);
	TimerWait(5000);

	TargetHeading = 32;
	GoToHeading(TargetHeading,10);
	TimerWait(100);
	GoToHeading(TargetHeading,10);
	TimerWait(5000);

	TargetHeading = 32;
	GoToHeading(TargetHeading,10);
	TimerWait(100);
	GoToHeading(TargetHeading,10);
	TimerWait(5000);

	TargetHeading = 32;
	GoToHeading(TargetHeading,8);
	TimerWait(100);
	GoToHeading(TargetHeading,3);
	TimerWait(5000);

	TargetHeading = 32;
	GoToHeading(TargetHeading,10);
	TimerWait(100);
	GoToHeading(TargetHeading,5);
	TimerWait(5000);

	TargetHeading = 32;
	GoToHeading(TargetHeading,10);
	TimerWait(100);
	GoToHeading(TargetHeading,5);
	TimerWait(5000);

	TargetHeading = 32;
	GoToHeading(TargetHeading,10);
	TimerWait(100);
	GoToHeading(TargetHeading,5);
	TimerWait(5000);

}
//##########################################################################################################





/**********************************************************************************************************
*
*	 Turn Test - Keeps Turning 64 from were it started.
*
***********************************************************************************************************/

//##########################################################################################################
void TurnTest(void)
{
	unsigned char	TargetHeading;


	TargetHeading	= ReadCompass() ? Heading + 64 : 0;

	while (!(getkey() == 1))
	{
		if(ReadCompass())
		{
			show1hex(Heading);

			if(Heading < TargetHeading)
			{
				SetRightMotorPWM(8);
				SetLeftMotorPWM(-8);
				show1hex(TargetHeading);
				//TimerWait(20);
			}
			if(Heading > TargetHeading)
			{
				SetRightMotorPWM(-8);
				SetLeftMotorPWM(8);
				show1hex(TargetHeading);
				//TimerWait(20);
			}
			if(-10 < (Heading-TargetHeading) && 10 > (Heading-TargetHeading))
			{
				LCD_puts_f(PSTR("Home"),1);
				TargetHeading    += 64;
				SetRightMotorPWM(0);
				SetLeftMotorPWM(0);
				TimerWait(3000);
			}
		}
		else
		{
			LCD_puts_f(PSTR("Error"),1);
		}
	}
}
//##########################################################################################################




/**********************************************************************************************************
*
*	 Line Sensor Test - Keeps Turning 64 from were it started.
*
***********************************************************************************************************/

//##########################################################################################################
void LineDisplay(void)
{
	LCD_puts_f(PSTR("Sending To Computer"),1);
	unsigned char adcLine;
	while (!(getkey() == 1))
	{
		adcLine = GetADC(FrontLineSensor);
		sendNumber(adcLine);
		sendString("\r\n");
		//TimerWait(200);
	}
}
//##########################################################################################################
